﻿#pragma once

#include <xy/geom/bezier.h>

namespace xy
{

namespace geom
{

template <std::size_t N, bool Rat = false> class bezier_curve : public bezier<1, N, Rat>
{
    using vecd_t = vec<1, double>;       // 参数域向量
    using vecr_t = vec<N + Rat, double>; // (wP,w)
    using vec_t = vec<N, double>;        // P
    using bezier<1, N, Rat>::m_cpts;

  public:
    using bezier<1, N, Rat>::degree;
    using bezier<1, N, Rat>::set_poles;
    using bezier<1, N, Rat>::pole_num;
    using bezier<1, N, Rat>::weight;
    using bezier<1, N, Rat>::set_weight;
    using bezier<1, N, Rat>::bound;

    /**
     * @brief BezierCurve 构造函数
     *
     * @param[in] cpts
     * @param[in] degree
     * @param[in] normalize
     */
    bezier_curve(std::vector<vecr_t> &&cpts, std::size_t degree, bool normalize = true)
        : bezier<1, N, Rat>(math::ndarray<1, vecr_t>{{std::move(cpts)}}, {{degree}}, normalize)
    {
    }

    /**
     * @brief 计算曲线上点的位置
     *
     * @param[in] param
     * @return vec_t
     */
    vec_t d0(const vecd_t &param) const override
    {
        auto p = de_casteljau1(m_cpts.data(), m_cpts.size(), param[0]);
        if constexpr (Rat)
            return homogeneous_to_cartesian(p);
        else
            return p;
    }

    /**
     * @brief 计算曲线上点的位置和导数
     *
     * @param[in] param
     * @return std::array<vec_t, 2>
     */
    std::array<vec_t, 2> d1(const vecd_t &param) const override
    {
        std::size_t d = degree();
        std::vector<double> knots((d + 1) * 2);
        std::fill(knots.begin(), knots.begin() + d + 1, 0.0);
        std::fill(knots.begin() + d + 1, knots.end(), 1.0);

        auto t = param[0];
        if constexpr (Rat)
        {
            auto ders = rat_curve_deriv(m_cpts.data(), knots.data(), knots.size(), degree(), 1, t);
            return {ders[0], ders[1]};
        }
        else
        {
            auto ders = curve_deriv_alg1(m_cpts.data(), knots.data(), knots.size(), degree(), 1, t);
            return {ders[0], ders[1]};
        }
    }

    /**
     * @brief 计算曲线上点的位置、导数和二阶导数
     *
     * @param[in] param
     * @return std::array<vec_t, 3>
     */
    std::array<vec_t, 3> d2(const vecd_t &param) const override
    {
        std::size_t d = degree();
        std::vector<double> knots((d + 1) * 2);
        std::fill(knots.begin(), knots.begin() + d + 1, 0.0);
        std::fill(knots.begin() + d + 1, knots.end(), 1.0);

        auto t = param[0];
        if constexpr (Rat)
        {
            auto ders = rat_curve_deriv(m_cpts.data(), knots.data(), knots.size(), degree(), 2, t);
            return {ders[0], ders[1], ders[2]};
        }
        else
        {
            auto ders = curve_deriv_alg1(m_cpts.data(), knots.data(), knots.size(), degree(), 2, t);
            return {ders[0], ders[1], ders[2]};
        }
    }

    /**
     * @brief 升次到 n 次 Bezier 曲线
     *
     * @param[in] n
     * @return true
     * @return false
     */
    bool elevate(std::size_t n)
    {
        if (n <= degree())
            return false;

        auto &m_degree = degree();
        auto Q = bezier_elevate(m_cpts.data(), m_cpts.size(), n - degree());

        m_degree = n;
        set_poles(std::move(Q));
        return true;
    }

    /**
     * @brief 降次 1 次
     *
     * @return double
     */
    double reduce()
    {
        if (degree() <= 1)
            return 0;

        auto &m_degree = degree();
        std::vector<vecr_t> Q(m_cpts.size() - 1);
        double tol = bezier_reduce(m_cpts.data(), m_cpts.size(), Q.data());

        m_degree--;
        set_poles(std::move(Q));
        return tol;
    }

    /**
     * @brief 在 t \in [0, 1] 处切割 Bezier 曲线为两个 Bezier 曲线
     *
     * @param[in] t
     * @return std::tuple<std::vector<vecr_t>, std::vector<vecr_t>>
     */
    std::tuple<std::vector<vecr_t>, std::vector<vecr_t>> split(double t) const
    {
        std::size_t n = degree();
        auto &P = m_cpts;
        std::vector<vecr_t> Q(n + 1);
        std::vector<vecr_t> R(n + 1);

        for (std::size_t i = 0; i < n + 1; ++i)
            R[i] = m_cpts.flatten()[i];
        for (std::size_t r = 0; r < n; r++)
        {
            Q[r] = R[0];
            for (std::size_t i = 0; i < n - r; i++)
                R[i] = R[i] * (1 - t) + R[i + 1] * t;
        }
        Q[n] = R[0];
        return {std::move(Q), std::move(R)};
    }

    /**
     * @brief 反转曲线
     *
     * @return void
     */
    void reverse()
    {
        std::reverse(m_cpts.flatten().begin(), m_cpts.flatten().end());
    }
};

template <std::size_t N> struct bezier_curve_qr : public bezier_curve<N, true>
{
    using vecr_t = vec<N + 1, double>; // (wP,w)
    using vec_t = vec<N, double>;      // P

  public:
    bezier_curve_qr(bezier_curve_qr &&) = delete;

    /**
     * @brief 三点构造
     *
     * @param[in] p0
     * @param[in] p1
     * @param[in] p2
     */
    bezier_curve_qr(const vecr_t &p0, const vecr_t &p1, const vecr_t &p2)
        : bezier_curve<N, true>({p0, p1, p2}, 2, false)
    {
    }

    /**
     * @brief 构造二次有理 Bezier 圆弧：w0 = w2 = 1, w1 > 0
     * w1 < 1 时为椭圆弧，w1 = 1 时为抛物线，w1 > 1 时为双曲线
     * 若 p1 位于 p0-p2 的中垂线上，则 w1 = cos(theta) 时得到圆心角为 2 * theta 的圆弧
     *
     * @param[in] p0
     * @param[in] p1
     * @param[in] p2
     */
    bezier_curve_qr(const vec_t &p0, const vecr_t &p1, const vec_t &p2)
        : bezier_curve<N, true>({cartesian_to_homogeneous(p0), p1, cartesian_to_homogeneous(p2)}, 2, false)
    {
    }

    /**
     * @brief 获得肩点位置
     *
     * @return vec_t
     */
    vec_t shoulder_point() const
    {
        return this->value(0.5);
    }

    /**
     * @brief 在肩点处切割圆弧，同时将连接处的权重归一
     *
     * @param[in] t
     * @return std::tuple<std::vector<vecr_t>, std::vector<vecr_t>>
     */
    std::tuple<std::vector<vecr_t>, std::vector<vecr_t>> split_at_shoulder_point() const
    {
        auto [Q, R] = bezier_curve<N, true>::split(0.5);
        auto w1 = this->weight(1);
        auto wq = std::sqrt((1 + w1) / 2);
        Q[1] *= wq / Q[1][N];
        R[1] *= wq / R[1][N];
        Q[2] /= Q[2][N];
        R[0] /= R[0][N];
        return std::make_tuple(Q, R);
    }

    /**
     * @brief 构造单个圆弧
     *
     * @param[in] P0 起点
     * @param[in] T0 起点切向
     * @param[in] P2 终点
     * @param[in] T2 终点切向
     * @param[in] P 圆弧上一点
     * @return std::shared_ptr<bezier_curve_qr>
     */
    static std::shared_ptr<bezier_curve_qr> make_one_arc(const vec_t &P0, const vec_t &T0, const vec_t &P2,
                                                         const vec_t &T2, const vec_t &P)
    {
        vecr_t R1;
        if (cross(T0, T2).modulus() > math::numeric::distance)
        {
            auto option1 = intersect_line_line(P0, T0, P2, T2);
            if (!option1.has_value())
                return nullptr;

            auto [alpha10, alpha12] = option1.value();
            auto P1 = P0 + T0 * alpha10;
            auto option2 = intersect_line_line(P1, P - P1, P0, P2 - P0);
            auto [alpha20, alpha22] = option2.value();

            auto a = std::sqrt(alpha22 / (1 - alpha22));
            auto u = a / (1 + a);
            auto num = (1 - u) * (1 - u) * dot(P - P0, P1 - P) + u * u * dot(P - P2, P1 - P);
            auto den = 2 * u * (1 - u) * (P1 - P).square_modulus();
            auto w1 = num / den;
            R1 = cartesian_to_homogeneous(P1) * w1;
        }
        else
        {
            auto option = intersect_line_line(P, T0, P0, P2 - P0);
            auto [alpha0, alpha2] = option.value();

            // alpha0 = (1 - u)^2, alpha2 = u^2
            auto a = std::sqrt(alpha2 / (1 - alpha2)); // a = u / (1 - u)
            auto u = a / (1 + a);
            auto b = 2 * u * (1 - u);  // 1 - b = [(1 - u) + u]^2 - 2u(1 - u) = (1 - u)^2 + u^2
            b = -alpha0 * (1 - b) / b; // 1 / f(u) = [(1 - u)^2 + u^2] / 2u(1 - u)

            R1 = cartesian_to_homogeneous(T0 * b);
            R1[N] = 0;
        }

        return std::make_shared<bezier_curve_qr>(P0, R1, P2);
    }
};

} // namespace geom

} // namespace xy